'''Date : O5th october 2012
	 Authors : Isaline Laurent (ens12ilt) - Gizem Berkdemir (ens12gbr)

	 This file contains all the motion functions, from computing the speed to request the server.
	 '''

HEADERS = {"Content-type": "application/json", "Accept": "text/json"}
URL = 'localhost:50000'

import json, time, httplib, sys
from math import pi, atan2
from tools import *
from path import *
from laser import *
global params

def setGoalPoint(jpath, position, limit) :
		''' Transform coordinate of computed goal point into vehicule coordinates 
					Input : jpath, a loaded json file containing the point of the path
						position, the robot position
						limit, the number of point into which pick up the carrot point
					Output : angle, the angle between the robot and the found carrot point
						 distance, the length of the arc subtended by the previous angle, between the robot and the carrot point
						 r, the radius of arc
		'''
		rpath = getConvertPath(jpath, position, limit) 
		d, i, angle, p = getNextPoint(rpath, params['LookAheadDistance'], limit)
		if (i == -1 ):
				return pi, 0, 0, p 
		else :
				carrot_point = rpath[i]
				r = getRadius(carrot_point, d)
				distance = r*angle 
				deleteObsPoints(jpath, i)
				return angle, distance, r, p



def getPosition() :
		''' Return the current position and orientation of the robot
          Output : a python object into which the json position has been loaded
    '''
		co = httplib.HTTPConnection(URL)
		co.request('GET','/lokarria/localization')
		response = co.getresponse()
		if (response.status == 200):
				position = response.read()
				response.close()
				return json.loads(position)
		else:
				return 

def sendOrder(angle,linear) :
		''' Send the moving order to the robot 
          Input : angle, the angular speed
                  linear, the linear speed
    '''
		co = httplib.HTTPConnection(URL)
		params = json.dumps({'TargetAngularSpeed':angle,'TargetLinearSpeed':linear})
		co.request('POST','/lokarria/differentialdrive',params,HEADERS)
		return 

def init(lookahead) :
		''' Initiate the global parameters about speed and position '''
		global params
		initLaser()
		co = httplib.HTTPConnection(URL)
		co.request('GET','/lokarria/differentialdrive')
		response = co.getresponse()
		if (response.status == 200):
				dd_params = response.read()
				response.close()
				params = json.loads(dd_params)['Properties']
				params['LookAheadDistance'] = lookahead 
		else:
				return 
		
		
def usage(argv) :
    ''' Define the usage of the program and exit'''  
    print "Usage : python ", argv[0], "file limit speed lookahead"
    print "\t\t limit > 0, 0 < speed <= 1, lookahead > 0"
    exit()

if __name__ == '__main__' :
		''' Input : file, path of the path file
                limit, number of element taken for each step into the path
                speed, factor of the maximum linear speed used
                lookahead, the lookahead distance the robot tend to join
    '''
		if len(sys.argv) != 5 :
			usage(sys.argv)
			
		file = sys.argv[1]
		limit = int(sys.argv[2])
		speed = float(sys.argv[3])
		lookahead = float(sys.argv[4])

		if limit <= 0 or speed <= 0 or speed > 1 or lookahead <= 0:
			usage(sys.argv)

		init(lookahead)
		path = loadPath(file)
		while len(path) > 0 :
				position = getPosition()
				angle, linear, radius, p = setGoalPoint(path, position, limit)
				if p == 1 and len(path) <= limit :
					del path[:]
					break

				warn_zone, laser = warnZone(angle, radius)
				if (warn_zone != -1) :
					angle = findAngle(laser, params['LookAheadDistance'], warn_zone)
					sendOrder(params['MaxAngularSpeed'], 0)
					time.sleep(abs(angle) / float(params['MaxAngularSpeed']))
					sendOrder(0,params['MaxLinearSpeed'])
					time.sleep(params['LookAheadDistance'] / (2 * float(params['MaxLinearSpeed'])))
				else :
					t, a, l = getSleepTime(angle, linear, radius, params, speed)
					sendOrder(a, l)
					time.sleep(t)
					
				sendOrder(0, 0)
